/**
 * 
 */
package physics;

import java.security.InvalidParameterException;

import javax.measure.quantity.Angle;
import javax.measure.quantity.Dimensionless;

import org.jscience.mathematics.vector.DenseMatrix;
import org.jscience.mathematics.vector.DenseVector;
import org.jscience.mathematics.vector.Matrix;
import org.jscience.mathematics.vector.Vector;
import org.jscience.physics.amount.Amount;

/**
 * @author Flavio
 *
 */
public class PoincareTransform implements Transform<PoincareTransform> {
	
	private Matrix<Amount<?>> LorentzMatrix;
	private Vector<Amount<?>> TranslationVector;
	
	public PoincareTransform() {
		this.LorentzMatrix = DenseMatrix.valueOf(new Amount<?>[][] {
				{ Amount.ONE, Amount.ZERO, Amount.ZERO, Amount.ZERO },
				{ Amount.ZERO, Amount.ONE, Amount.ZERO, Amount.ZERO },
				{ Amount.ZERO, Amount.ZERO, Amount.ONE, Amount.ZERO },
				{ Amount.ZERO, Amount.ZERO, Amount.ZERO, Amount.ONE }
				});
		this.TranslationVector = DenseVector.valueOf(new Amount<?>[] 
				{ Amount.ZERO, Amount.ZERO, Amount.ZERO, Amount.ZERO } );
	}
	
	public PoincareTransform(PoincareTransform toCopy) {
		this.LorentzMatrix = toCopy.getLorentzMatrix();
		this.TranslationVector = toCopy.getTranslationVector();
	}
	
	public Matrix<Amount<?>> getLorentzMatrix() {
		return this.LorentzMatrix;
	}
	
	public Vector<Amount<?>> getTranslationVector() {
		return this.TranslationVector;
	}
	
	public PoincareTransform copyOf(PoincareTransform toCopy) {
		this.LorentzMatrix = toCopy.getLorentzMatrix();
		this.TranslationVector = toCopy.getTranslationVector();
		return this;
	}
	

	@Override
	public PoincareTransform identity() {
		return new PoincareTransform();
	}
	
	@Override
	public PoincareTransform inverse() {
		PoincareTransform copyOfThis = new PoincareTransform(this);
		this.LorentzMatrix = this.LorentzMatrix.inverse();
		this.TranslationVector = this.LorentzMatrix.times(this.TranslationVector).opposite();
		PoincareTransform toReturn = new PoincareTransform(this);
		this.copyOf(copyOfThis);
		return toReturn;
	}

	@Override
	public PoincareTransform composedWith(PoincareTransform toCompose) {
		PoincareTransform copyOfThis = new PoincareTransform(this);
		this.LorentzMatrix = toCompose.getLorentzMatrix().times(this.LorentzMatrix);
		this.TranslationVector = toCompose.getLorentzMatrix().times(this.TranslationVector).plus(toCompose.getTranslationVector());
		PoincareTransform toReturn = new PoincareTransform(this);
		this.copyOf(copyOfThis);
		return toReturn;
	}
	
	public PoincareTransform threeBetaComponentsBoost(Amount<Dimensionless> beta_x, 
			Amount<Dimensionless> beta_y,
			Amount<Dimensionless> beta_z) {
		if ( beta_x.isGreaterThan(Amount.ZERO) && beta_x.isLessThan(Amount.ONE) &&
				beta_y.isGreaterThan(Amount.ZERO) && beta_y.isLessThan(Amount.ONE) &&
				beta_z.isGreaterThan(Amount.ZERO) && beta_z.isLessThan(Amount.ONE) ) {
			throw new InvalidParameterException();
		}
		Amount<?> beta2 = beta_x.times(beta_x).plus(beta_y.times(beta_y)).plus(beta_z.times(beta_z));
		Amount<?> gamma = Amount.ONE.minus(beta2).sqrt().inverse();
		DenseMatrix<Amount<?>> M = DenseMatrix.valueOf(new Amount<?>[][] {
				{ gamma, gamma.times(beta_x).opposite(), gamma.times(beta_y).opposite(), gamma.times(beta_z).opposite() },
				{ gamma.times(beta_x).opposite(), Amount.ONE.plus(Amount.ONE.minus(gamma).times(beta_x).times(beta_x).divide(beta2)), Amount.ONE.minus(gamma).times(beta_x).times(beta_y).divide(beta2), Amount.ONE.minus(gamma).times(beta_x).times(beta_z).divide(beta2) },
				{ gamma.times(beta_y).opposite(), Amount.ONE.minus(gamma).times(beta_y).times(beta_x).divide(beta2), Amount.ONE.plus(Amount.ONE.minus(gamma).times(beta_y).times(beta_y).divide(beta2)), Amount.ONE.minus(gamma).times(beta_y).times(beta_z).divide(beta2) },
				{ gamma.times(beta_z).opposite(), Amount.ONE.minus(gamma).times(beta_z).times(beta_x).divide(beta2), Amount.ONE.minus(gamma).times(beta_z).times(beta_y).divide(beta2), Amount.ONE.plus(Amount.ONE.minus(gamma).times(beta_z).times(beta_z).divide(beta2)) }
		});
		LorentzMatrix = M.times(LorentzMatrix);
		return this;
	}
	
	public PoincareTransform axisAngleRotation(Amount<Dimensionless> n_x, Amount<Dimensionless> n_y, Amount<Dimensionless> n_z, 
			Amount<Angle> theta) {
		/* TODO
		Amount<?> n2 = n_x.pow(2).plus(n_y.pow(2)).plus(n_z.pow(2));
		Amount<?> kx = n_x.divide(n2);
		Amount<?> ky = n_y.divide(n2);
		Amount<?> kz = n_z.divide(n2);
		*/
		return this;
	}

	@Override
	public FourVector<?, PoincareTransform> transform(FourVector<?, PoincareTransform> toTransform) {
		Vector<Amount<?>> v = new FourVector<>(toTransform);
		v = this.LorentzMatrix.times(toTransform).plus(this.TranslationVector);
		return new FourVector<>(v.get(0), v.get(1), v.get(2), v.get(3));
	}
	
}
